#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <fstream>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace cv;
using namespace std;

#define calibration

int CHECKERBOARD[2]{ 15,19 };

int main()
{
    ifstream fin("right_img.txt");             /* 标定所用图像文件的路径 */
    ofstream fout("caliberation_result_right.txt");  /* 保存标定结果的文件 */

    // 读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
    Size image_size;      /* 图像的尺寸 */
    Size board_size = Size(19, 15);           /* 标定板上每行、列的角点数 */
    vector<Point2f> image_points_buf;         /* 缓存每幅图像上检测到的角点 */
    vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */
    string filename;      // 图片名
    vector<string> filenames;
    std::string modelPath = "./image/model/*.bmp";
    std::vector<cv::String> images;
    cv::glob(modelPath, images);
    cv::Mat frame, gray;
    double Kernelsize_cali[2]{3.0, 3.0};      /* 标定板的宽度: 我们使用的是3.0mm * 3.0mm */

    std::vector<std::vector<cv::Point3f> > objpoints;  /* 真实的坐标点，通过横轴第一个点相对应起来的 */
    std::vector<cv::Point3f> objp;        
    
    for (int i{ 0 }; i < CHECKERBOARD[1]; i++)
    {
        for (int j{ 0 }; j < CHECKERBOARD[0]; j++)
        {
            objp.push_back(cv::Point3f(j * Kernelsize_cali[0], i * Kernelsize_cali[1], 0));
            cout << "(x,y,z):(" << j << "," << i << "," << 0 << ")" << endl;
        }
        cout << "one success!" << endl;
    }

    for (size_t i{ 0 }; i < images.size(); i++)
    {
        frame = cv::imread(images[i], 1);
        if (frame.empty()){ continue; }
        cout << "the current image is " << i << "th" << endl;
        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);

        //交点提取
        bool success = cv::findChessboardCorners(gray, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), image_points_buf, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
        if (success)
        {
            // 终止条件达到 阈值 或 最大次数 时终止
            cv::TermCriteria criteria(TermCriteria::EPS | TermCriteria::Type::MAX_ITER, 30, 0.001);
            
            // 为给定的二维点细化像素坐标
            cv::cornerSubPix(gray, image_points_buf, cv::Size(11, 11), cv::Size(-1, -1), criteria);

            // 在棋盘上显示检测到的角点
            cv::drawChessboardCorners(frame, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), image_points_buf, success);

            objpoints.push_back(objp);
            image_points_seq.push_back(image_points_buf);
        }
        else
        {
            cout << "some errors happened in cornerSubPix" << endl;
        }
    }

    image_size = cv::Size(frame.cols, frame.rows);
    int CornerNum = board_size.width * board_size.height;  // 每张图片上总的角点数
    (void)CornerNum; // 标注未使用的变量

    //-------------_以下是摄像机标定------------------

    Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 摄像机内参数矩阵 */
    vector<int> point_counts;   /* 每幅图像中角点的数量 */ 
    Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));       /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
    vector<Mat> tvecsMat;       /* 每幅图像的旋转向量 */
    vector<Mat> rvecsMat;       /* 每幅图像的平移向量 */

    // 初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 
    for (size_t i = 0; i < images.size(); i++)
    {
        point_counts.push_back(board_size.width * board_size.height);
    }

    /* 开始标定 */
    // object_points 世界坐标系中的角点的三维坐标
    // image_points_seq 每一个内角点对应的图像坐标点
    // image_size 图像的像素尺寸大小
    // cameraMatrix 输出，内参矩阵
    // distCoeffs 输出，畸变系数
    // rvecsMat 输出，旋转向量
    // tvecsMat 输出，位移向量
    // 0 标定时所采用的算法
    
    double error = calibrateCamera(objpoints, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);
    (void)error; // 标注未使用的变量

    //------------------------标定完成------------------------------------

    // -------------------对标定结果进行评价------------------------------

    double total_err = 0.0;         /* 所有图像的平均误差的总和 */
    double err = 0.0;               /* 每幅图像的平均误差 */
    vector<Point2f> image_points2;  /* 保存重新计算得到的投影点 */
    fout << "每幅图像的标定误差：\n";

    for (size_t i = 0; i < images.size(); i++)
    {
        vector<Point3f> tempPointSet = objpoints[i];

        /* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
        // 重投影验证，通过标定板坐标系实际角点—>重投影（外参矩阵）—>像素坐标系下的坐标点
        projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);

        // 计算新的投影点和旧的投影点之间的误差
        vector<Point2f> tempImagePoint = image_points_seq[i];
        Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
        Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);

        for (size_t j = 0; j < tempImagePoint.size(); j++)
        {
            image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
            tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
        }

        //求真实角点像素和重投影像素坐标两者之间的L2范数
        err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
        total_err += err /= point_counts[i];
        fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
    }
    fout << "总体平均误差：" << total_err / images.size() << "像素" << endl << endl;

    //-------------------------评价完成---------------------------------------------

    //-----------------------保存定标结果------------------------------------------- 
    Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 保存每幅图像的旋转矩阵 */
    fout << "相机内参数矩阵：" << endl;
    fout << cameraMatrix << endl << endl;
    fout << "畸变系数：\n";
    fout << distCoeffs << endl << endl << endl;

    //每个不同位置的图像都有自己的外参转换矩阵
    for (size_t i = 0; i < images.size(); i++)
    {
        fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
        fout << tvecsMat[i] << endl;

        // 将旋转向量转换为相对应的旋转矩阵 
        Rodrigues(tvecsMat[i], rotation_matrix);
        fout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
        fout << rotation_matrix << endl;
        fout << "第" << i + 1 << "幅图像的平移向量：" << endl;
        fout << rvecsMat[i] << endl << endl;
    }
    fout << endl;

    //--------------------标定结果保存结束-------------------------

    //----------------------显示定标结果---------------------------

    Mat mapx = Mat(image_size, CV_32FC1);
    Mat mapy = Mat(image_size, CV_32FC1);
    Mat R = Mat::eye(3, 3, CV_32F);
    string imageFileName;
    std::stringstream StrStm;
    for (size_t i = 0; i < images.size(); i++)
    {
        initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
        Mat imageSource = imread(images[i]);
        Mat newimage = imageSource.clone();
        remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
        StrStm.clear();
        imageFileName.clear();
        StrStm << i + 1;
        StrStm >> imageFileName;
        imageFileName += "./images/result/_d.jpg";
        imwrite(imageFileName, newimage);
    }

    fin.close();
    fout.close();
}
